#!/ust/bin/env python3
# coding: utf-8
"""
last edited at 2019-8-18
by Stone at BlueNet

最顶层模块，统一执行各个模块的初始化，并组装。


机器人的整体任务，可分解为找球-踩点-投球的子任务，上一任务完成后即切换到下一任务，很符合state pattern的描述。
本来也打算用state pattern写一个task.py 和 Task类出来，或许还可以提供一定的灵活性。但是每项task都涉及到recognizer，
controller的调用，如果单独写一个task.py，其命名空间内就没有这些组件，而如果把Task写在当前模块，则显得模块职责
不够清晰。在屋子里坐了一天时间，庶竭驽钝，想不出来什么好的解决方法。与其耗费时间，不如就放弃面向对象的方法，纯粹
用面向过程的方法来做吧。目前任务数量不多的情况下，这种方法的可读性可能更好一些。
"""
import logging
from copy import copy, deepcopy
from enum import unique, auto, IntEnum
from multiprocessing import Process

import config as cfg
import controller as ctrl
import recognizer as recog
import robot_state
from utils import dis


# ===== ===== ===== ===== =====
# GLOBAL CONSTANTS
# ===== ===== ===== ===== =====
# 把变量复制一遍，一是万一变量被错误修改，可以把范围缩小到模块之内，另外，用起来也方便。（似乎没什么作用，就是拿来浪费内存的）
POS_PAW = deepcopy(cfg.POS_PAW)
TOL_PAW_BALL = deepcopy(cfg.TOL_PAW_BALL)


@unique
class Steps(IntEnum):
    """所执行的子任务的编号。"""
    MV_TO_BALL = auto()  # move to the ball
    PICK_BALL = auto()   # pick up the ball
    MV_TO_CAST = auto()  # move to at where cast the ball
    CAST = auto()        # cast the ball


# ===== ===== ===== ===== =====
# FUNCTIONS
# ===== ===== ===== ===== =====
def init():
    """
    把初始化代码放进函数是为了方便被其他模块导入，也方便控制是否要初始化。
    然而放进函数之后，有些全局变量一个个global可能影响可读性，另一方面
    也妨碍PyCharm做语法检查。因此就直接把init的代码塞进loop了，免去global
    的麻烦。
    """
    pass


# TODO: to construct a context manager
def loop():
    logging.basicConfig(level=logging.DEBUG)
    logging.info('Robot start... \r\n\r\n\r\n')

    # 由于需要创建被所有进程共享的state对象，需要最先初始化。
    # 已在robot_init()中运行manager.start()
    robot_state.init()

    state = robot_state.manager.get_state()

    # #########################
    # 开始组装 robot
    # #########################
    comm_ser = ctrl.Communication(port=copy(cfg.SERIAL_PORT))
    effector = ctrl.Effector(comm_ser)
    controller = ctrl.MotionController()

    # 创建识别进程，并启动
    proc_recog = Process(name="proc-recog", target=recog.loop, args=(state,))
    proc_recog.start()

    # #########################
    # 启动 robot !!!
    # #########################
    curr_step = -1  # current step; jump to else
    while True:
        # TODO: control the rate of looping
        if curr_step == Steps.MV_TO_BALL:
            state.set_recog_task(recog.TASK_RECOG_BALL)  # 设置识别任务
            pos, is_ball_new = state.get_pos_ball()      # 接收识别结果（此时不一定已完成识别）

            if not is_ball_new:  # 当坐标未更新时，挡住不执行后续步骤
                continue

            # 计算角速度，速度
            w, v = controller.feedback_control(pos)
            effector.rotate(w)
            effector.mv_fb(v)

            # 当球的坐标和机械爪中心的距离小于容许误差时，跳入下一步骤
            if dis(*pos, *POS_PAW) < TOL_PAW_BALL:
                curr_step = next(steps)

        elif curr_step == Steps.PICK_BALL:
            curr_step = next(steps)

        elif curr_step == Steps.MV_TO_CAST:
            curr_step = next(steps)

        else:
            steps = Steps.__members__.values().__iter__()
            curr_step = next(steps)


if __name__ == '__main__':
    loop()
